#ifndef M3508_H
#define M3508_H
#include "../src/Logger.h"
#include <linux/can.h>
#include "CanDriver.h"
#include "Algorithm/PIDController.h"
#include "../src/utils.hpp"

namespace MM {
    using std::array, std::vector, std::string;
    using std::unique_ptr;
    using std::optional, std::nullopt;
    using std::function;
    using bq::log;

    struct M3508RawStatus {
        explicit operator can_frame() const;

        explicit M3508RawStatus(const can_frame &frame);

        [[nodiscard]] string format() const;

        uint16_t canId;
        uint16_t ecd; // before reducer 0~8191
        int16_t rpm;
        int16_t current; //mA
        uint8_t temperature;
    };


    struct M3508RawControl {
        explicit operator can_frame() const;

        explicit M3508RawControl(const can_frame &frame);

        explicit M3508RawControl(const array<int16_t, 4> &&ampere, uint16_t canId);

        uint16_t canId;
        array<int16_t, 4> ampere{};
    };

    struct M3508Status {
        uint16_t last_ecd;
        uint16_t ecd;
        float angle_single_round; // 单圈角度
        float angular; // 角速度,单位为:度/秒
        int16_t real_current; // 实际电流
        uint8_t temperature; // 温度 Celsius
        float total_angle; // 总角度,注意方向
        int32_t total_round; // 总圈数,注意方向
        int16_t output_current; //PID计算完后输出电流
    };

    enum {
        AngularLoop = 0,
        CurrentLoop = 1,
    };

    enum {
        StepFunc = 0,
        SineFunc = 1,
    };

    class M3508s {
    public:
        M3508s() = delete;

        ~M3508s();

        explicit M3508s(string canInterface, optional<string> &&loggerId = nullopt,
                        bool loggingToTextfile = true,
                        bool loggingToConsole = false);

        void posVelControl(array<optional<float>, 8> &&pos, array<float, 8> &&vel,
                           optional<array<PIDInstance, 8> > &&pos_pid = nullopt,
                           optional<array<PIDInstance, 8> > &&vel_pid = nullopt);


        void velControl(array<float, 8> &&vel, optional<array<PIDInstance, 8> > &&vel_pid = nullopt);

        template<bool Plotting = DisablePlotting>
        void enable();

        void stop() const;

        template<bool Loop, bool Func>
        void PIDCalibration(uint8_t motorId);

    private:
        [[nodiscard]] inline M3508RawStatus transformStatus(const can_frame &frame) const;

        static void fuse_check(const M3508RawStatus &status);

        void export_CSV(vector<pair<float, float> > &data) const;

        array<M3508Status, 8> statuses{};
        array<PIDInstance, 8> ang_pids{}, pos_pids{}, cur_pids{};
        array<optional<float>, 8> pos_ref{};
        array<float, 8> vel_ref{};
        SpinLock status_lock;
        SpinLock pid_lock;
        string interface;
        string id;
        log logger;
        unique_ptr<canpp::CanDriver<> > can_driver = nullptr;
    };
}

#endif //M3508_H
